/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;
import java.util.Vector;

/**
 * debug messages for robot
 * @author robert
 */
public class PhoenixDebug {
    PhoenixDrive drive;
    PhoenixControls controls;
    PhoenixSensors sensors;
    PhoenixLifter lifter;
    PhoenixBcd autonomousSwitch;
    PhoenixAutonomous autonomous;
    PhoenixArm arm;
    PhoenixMinibot minibot;
    Vector debugMessages = new Vector(0);

    /**
     * debug constructor
     * @param drive
     * @param controls
     * @param sensors
     * @param lifter
     * @param arm
     * @param autonomousSwitch
     * @param autonomous
     */
    public PhoenixDebug(PhoenixDrive drive, PhoenixControls controls, 
            PhoenixSensors sensors, PhoenixLifter lifter, PhoenixArm arm,
            PhoenixBcd autonomousSwitch, PhoenixAutonomous autonomous,
            PhoenixMinibot minibot) {
        this.drive = drive;
        this.controls = controls;
        this.sensors = sensors;
        this.lifter = lifter;
        this.autonomousSwitch = autonomousSwitch;
        this.autonomous = autonomous;
        this.arm = arm;
        this.minibot = minibot;
    }

    public void addMessage(String message) {
        debugMessages.addElement(message);
    }

    public void print() {
        for(int i = 0; i < debugMessages.size(); i++) {
            System.out.println("debug: " + debugMessages.elementAt(i));
        }
        debugMessages = new Vector(0);
    }

    /**
     * motor and throttle values
     */
    public void motors() {
        addMessage("left motor: " + drive.getPowerLeft() +
                " right motor: " + drive.getPowerRight() + " throttle: " +
                controls.throttleLinearized());
    }

    /**
     * debug for single motor
     */
    public void motor(SpeedController motor) {
        addMessage("motor value: " + motor.get());
    }

    /**
     * joystick values
     */
    public void joysticks() {
        addMessage("left joystick x: " +
                controls.leftStick.getAxis(Joystick.AxisType.kY) + " y: " +
                controls.leftStick.getAxis(Joystick.AxisType.kX));
        addMessage("right joystick x: " +
                controls.rightStick.getAxis(Joystick.AxisType.kY) + " y: " +
                controls.rightStick.getAxis(Joystick.AxisType.kX));
    }

    public void gamepad() {
        addMessage("left stick x: " +
                controls.gamePad.getAxis(Joystick.AxisType.kY) + " y: " +
                controls.gamePad.getAxis(Joystick.AxisType.kX));
        addMessage("right stick x: " +
                controls.gamePad.getAxis(Joystick.AxisType.kZ) + " y: " +
                controls.gamePad.getAxis(Joystick.AxisType.kThrottle));
    }

    /**
     * encoder values
     */
    public void encoders() {
        addMessage("left encoder: " + drive.getEncoderLeft() +
                " right encoder: " + drive.getEncoderRight());
    }

    /**
     * light sensor values
     */
    public void lightSensors() {
        addMessage("light sensor left: " + sensors.getLightLeft() +
                " middle: " + sensors.getLightMiddle() + " right: " +
                sensors.getLightRight());
    }

    /**
     * lifter value
     */
    public void lifter() {
        for(int i = 0; i < lifter.debugMessages.size(); i++) {
            addMessage("lifter: " + lifter.debugMessages.elementAt(i));
        }
        lifter.debugMessages = new Vector(0);

        addMessage("lifter height: " + lifter.getHeight() +
                " power: " + lifter.getPower());
    }


    /**
     * arm pot value, and whether it is opened
     */
    public void arm() {
        for(int i = 0; i < arm.debugMessages.size(); i++) {
            addMessage("arm: " + arm.debugMessages.elementAt(i));
        }
        arm.debugMessages = new Vector(0);

        addMessage("arm pot: " + arm.getHeight() +
                " claw opened: " + arm.getClaw());
    }

    /**
     * BCD value
     */
    public void BCD() {
        if(autonomousSwitch.getIfDifferent() != -1) {
            addMessage("autonomous switch: " +
                    autonomousSwitch.getIfDifferent());
        }
    }

    /**
     * autonomous time and current stage
     */
    public void autonomous() {
        for(int i = 0; i < autonomous.debugMessages.size(); i++) {
            addMessage("autonomous: " + autonomous.debugMessages.elementAt(i));
        }
        autonomous.debugMessages = new Vector(0);
    }

    /**
     * limit switch messages
     */
    public void limits() {
        addMessage("lifter limits top: " + lifter.lifterTopLimit.get() +
                " bottom: " + lifter.lifterBottomLimit.get());
        addMessage("claw limits top: " + arm.clawTopLimit.get() +
                " bottom: " + arm.clawBottomLimit.get());
    }



    public void minibot() {
        for(int i = 0; i < minibot.debugMessages.size(); i++) {
            addMessage("minibot: " + minibot.debugMessages.elementAt(i));
        }
        minibot.debugMessages = new Vector(0);
    }
}
